import processing.core.*;
import java.lang.Math;

class Vehicle {
    //color mColor;
    public Route mRoute;
    public float[] curPosition;
    public float[] mapPosition;
    public String mColor;
    public PVector mSpeed;
    public int mType;
    public float ratio = 1.2f;
    
    private float[] topleft;
    private float width;
    private float height;    
    private int index;
    private float[][] screenPositions;
    private static float epsilon = 0.001f;
    // Constructors
    
    public Vehicle(Route route)
    {
        mRoute = route;
        screenPositions = mRoute.screenPositions;
        curPosition = new float[2];
        curPosition[0] = screenPositions[0][0];
        curPosition[1] = screenPositions[0][1];
        float tmpx = screenPositions[1][0] - screenPositions[0][0];
        float tmpy = screenPositions[1][1] - screenPositions[0][1];
        mSpeed = new PVector(tmpx, tmpy);
        mSpeed.normalize();
        mSpeed.mult(ratio);
        //System.out.println("init mSpeed " + mSpeed.x + " " + mSpeed.y);
        index = 1;
        topleft = new float[2];
        topleft[0] = 0;
        topleft[1] = 0;
        mapPosition = new float[2];
        width = 800;
        height = 800;
    }
    
    public Vehicle(Route route, float[] pt, float w, float h)
    {
        mRoute = route;
        screenPositions = mRoute.screenPositions;
        curPosition = new float[2];
        curPosition[0] = screenPositions[0][0];
        curPosition[1] = screenPositions[0][1];
        float tmpx = screenPositions[1][0] - screenPositions[0][0];
        float tmpy = screenPositions[1][1] - screenPositions[0][1];
        mSpeed = new PVector(tmpx, tmpy);
        mSpeed.normalize();
        mSpeed.mult(ratio);
        //System.out.println("init mSpeed " + mSpeed.x + " " + mSpeed.y);
        index = 1;
        topleft = new float[2];
        topleft[0] = pt[0];
        topleft[1] = pt[1];
        mapPosition = new float[2];
        width = w;
        height = h;
    }
    
    public void freeze()
    {
        float[]xy = mRoute.getLocationFromScreenPosition(curPosition);
        mapPosition[0] = xy[0];
        mapPosition[1] = xy[1];
        
    }
    
    public void move()
    {  
        //System.out.println("Index " + index);
        //screenPositions = mRoute.screenPositions;
        if (isOnline(screenPositions[index-1],
                     screenPositions[index],
                     curPosition)) {
            curPosition[0] += mSpeed.x;
            curPosition[1] += mSpeed.y;
        } else {
            ++index;
            //System.out.println("seg end new index " + index);
            if (index == screenPositions.length) {
                resetVehicle();
                //System.out.println("route end reset");
                return;
            }
            setCurPosition(screenPositions[index-1]);
            updateSpeed(screenPositions[index],
                        screenPositions[index-1]);
        }
    }
    
    private float getSqrtDistance(float[] p1, float[] p2)
    {
        return (float)java.lang.Math.sqrt((double)(p1[0] - p2[0]) * (p1[0] - p2[0]) + 
            (double)(p1[1] - p2[1]) * (p1[1] - p2[1]));
    }
    
    private float getSqrtDistance(float[] p, PVector p2)
    {
        return (p[0] - p2.x) * (p[0] - p2.x) + (p[1] - p2.y) * (p[1] - p2.y);
    }
    
    boolean isOnline(PVector pt, PVector startPos, PVector endPos)
    {
        float dis1 = startPos.dist(pt);
        float dis2 = pt.dist(endPos);
        float disa = startPos.dist(endPos);
        //println("dis1 " + dis1 + " dis2 " + dis2 + " disa " + disa);
        if (java.lang.Math.abs(dis1 + dis2 - disa) < epsilon) return true;
        else return false;
    }
    
    private boolean isOnline(float[] ps, float[] pe, float[] p)
    {
        float disa = getSqrtDistance(ps, pe);
        float dis1 = getSqrtDistance(ps, p);
        float dis2 = getSqrtDistance(pe, p);
        /*
        System.out.println("curPos: " + p[0] + " " + p[1]);
        System.out.println("ps: " + ps[0] + " " + ps[1]);
        System.out.println("pe: " + pe[0] + " " + pe[1]);
        System.out.println("speed " + mSpeed.x + " " + mSpeed.y);
        System.out.println("disa: " + disa + " dis1: " + dis1 + " dis2: " + dis2 + 
                           " dis1 + dis2 " + (dis1 + dis2));
        */
        if (java.lang.Math.abs(dis1 + dis2 - disa) < epsilon) return true;
        else return false;
    }
    
    private void updateSpeed(PVector startPos, PVector endPos)
    {
        mSpeed = new PVector(endPos.x - startPos.x, endPos.y - startPos.y);
        mSpeed.normalize();
        mSpeed.mult(ratio);
    }
    
    private void updateSpeed(float[] to, float[] from)
    {
        mSpeed = new PVector(to[0] - from[0], to[1] - from[1]);
        //mSpeed.x = to[0] - from[0];
        //mSpeed.y = to[1] - from[1];
        mSpeed.normalize();
        mSpeed.mult(ratio);
    }
    
    public void recalculateCurPosition()
    {
        float[] xy = mRoute.getScreenPositionFromLocation(mapPosition);
        curPosition[0] = xy[0];
        curPosition[1] = xy[1];
    }
    
    public void setCurPosition(float[] pos)
    {
        curPosition[0] = pos[0];
        curPosition[1] = pos[1];
    }
    
    public void resetVehicle()
    {
        index = 1;
        float tmpx = screenPositions[1][0] - screenPositions[0][0];
        float tmpy = screenPositions[1][1] - screenPositions[0][1];
        mSpeed.x = tmpx;
        mSpeed.y = tmpy;
        mSpeed.normalize();
        mSpeed.mult(ratio);
        curPosition[0] = screenPositions[0][0];
        curPosition[1] = screenPositions[0][1];
    }
}
